/*
 * @Description: 闭环检测的具体实现
 * @Author: Sang Hao
 * @Date: 2021-09-27 17:27:26
 * @LastEditTime: 2021-10-27 14:09:34
 * @LastEditors: Sang Hao
 */

#include <pcl/io/pcd_io.h>
#include "glog/logging.h"

#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/mapping/loop_closing/loop_closing.hpp"
#include "lidar_slam/models/cloud_filter/voxel_filter.hpp"
#include "lidar_slam/models/registration/ndt_registration.hpp"
#include "lidar_slam/models/cloud_filter/no_filter.hpp"

namespace lidar_slam {
/**
 * @description: public构造函数
 * @param  {*}
 * @return {*}
 */
LoopClosing::LoopClosing() {
	InitWithConfig();
}
/**
 * @description: 总的初始化函数，对该类的私有变量进行赋值，内部会调用其他初始化函数
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::InitWithConfig() {
	std::string config_file_path = WORK_SPACE_PATH + "/config/loop_closing.yaml";
	YAML::Node config_node = YAML::LoadFile(config_file_path);

	std::cout << "----------------闭环检测初始化------------------" << std::endl;
	InitParam(config_node);
	InitDataPath(config_node);
	InitRegistration(registration_ptr_, config_node);
	InitFilter("map", map_filter_ptr_, config_node);
	InitFilter("scan", scan_filter_ptr_, config_node);

	return true;
}
/**
 * @description: 初始化一些参数，如扩展帧数量等 
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::InitParam(const YAML::Node& config_node) {
	extend_frame_num_ = config_node["extend_frame_num"].as<int>();
	loop_step_ = config_node["loop_step"].as<int>();
	diff_num_ = config_node["diff_num"].as<int>();
	detect_area_ = config_node["detect_area"].as<float>();
	fitness_score_limit_ = config_node["fitness_score_limit"].as<float>();
	
	return true;
}

/**
 * @description: 初始化保存文件路径
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::InitDataPath(const YAML::Node& config_node) {
	std::string data_path = config_node["data_path"].as<std::string>();
	if (data_path == "./") {
		data_path = WORK_SPACE_PATH;
	}
	key_frames_path_ = data_path + "/slam_data/key_frames";

	return true;
}

/**
 * @description: 初始化匹配方法
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::InitRegistration(std::shared_ptr<RegistrationInterface>& registration_ptr, 
	const YAML::Node& config_node) {
	std::string registration_method = config_node["registration_method"].as<std::string>();
	if (registration_method == "NDT") {
		registration_ptr_ = 
			std::make_shared<NDTRegistration>(config_node[registration_method]);
	} else {
		return false;
	}

	return true;
}

/**
 * @description: 初始化滤波方法
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::InitFilter(std::string filter_user, 
	std::shared_ptr<CloudFilterInterface>& filter_ptr,
	const YAML::Node& config_node) {
	std::string filter_method = config_node[filter_user + "_filter"].as<std::string>();
	std::cout << filter_user << "选择的滤波方法为:" << filter_method << std::endl;
	if (filter_method == "voxel_filter") {
		filter_ptr = std::make_shared<VoxelFilter>(config_node[filter_method][filter_user]);	
	} else if (filter_method == "no_filter") {
		filter_ptr = std::make_shared<NoFilter>();
	} else {
		LOG(ERROR) << "没有为 " << filter_user << " 找到与 " << filter_method << " 相对应的滤波方法!";
		return false;
	}
	return true;
}

/**
 * @description: public函数，回环检测更新，具体实现在DetectNearestKeyFrame和CloudRegistration
 * @param  {*}
 * @return {*}
 * @param {KeyFrame} key_frame
 */
bool LoopClosing::Update(const KeyFrame key_frame) {
	has_new_loop_pose_ = false;
	
	all_key_frames_.push_back(key_frame);
	
	int key_frame_index = 0;
	// 先找近邻，也就是设置三种跳跃情况，如果没有近邻，则没必要去做回环检测了
	if (!DetectNearestKeyFrame(key_frame_index)) {
		return false;
	}
	// 再用匹配的方法找回环
	if (!CloudRegistration(key_frame_index)) {
		return false;
	}
	has_new_loop_pose_ = true;
	return true;
}

/**
 * @description: 设置检测的三种跳跃情况，以便快速检测，同时设置了检测到回环的条件
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::DetectNearestKeyFrame(int& key_frame_index) {
	static int skip_cnt = 0;
	static int skip_num = loop_step_;
	// 情况二：每隔skip_num检测一次，其中skip_num至少为loop_step_
	if (++skip_cnt < skip_num) {
		return false;
	}
	// 如果是刚开始，还没够diff_num_呢，也没必要去检测
	if ((int)all_key_frames_.size() < diff_num_ + 1) {
		return false;
	}
	int key_num = (int)all_key_frames_.size();
	float min_distance = 10000.0;
	float distance = 0.0;

	KeyFrame history_key_frame;
	KeyFrame current_key_frame = all_key_frames_.back();

	// 最小距离的帧索引，函数参数
	key_frame_index = -1;

	for (int i = 0; i < key_num - 1; ++i) {
		// 情况一：距离当前帧太近，时间差太小，不检测，没意义
		if (key_num - i < diff_num_) {
			break;
		}
		history_key_frame = all_key_frames_.at(i);
		distance = 	fabs(current_key_frame.pose(0, 3) - history_key_frame.pose(0, 3)) + 
					fabs(current_key_frame.pose(1, 3) - history_key_frame.pose(1, 3)) + 
	   				fabs(current_key_frame.pose(2, 3) - history_key_frame.pose(2, 3));
		if (distance < min_distance) {
			min_distance = distance;
			key_frame_index = i;
		}
	}
	// 如果小于扩展数，则该关键帧都无法左右扩展得到一个局部地图，因此也不考虑
	if (key_frame_index < extend_frame_num_) {
		return false;
	}
	skip_cnt = 0;
	skip_num = (int)min_distance;
	// 情况三：根据最小距离来重新计算等待时间
	// 如果最小距离都比detect_area_大，那么有理由相信，
	// 在接下来的min_distance - detect_area_距离内不会出现闭环
	// 因此可以基于最小距离设置大跳跃次数skip_num
	if (min_distance > detect_area_) {
		skip_num = std::max((int)(min_distance / 2.0), loop_step_);
		return false;
	} else {
		// 检测到回环的近邻区了，更新skip_num为初始的loop_step_，隔loop_step_再检测一次
		skip_num = loop_step_;
		return true;
	}
}

/**
 * @description: 通过匹配的方法，在近邻区判断是否真的存在回环。其中用到匹配的两个点云。
 * 					匹配的点云调用JointMap和JointScan得到
 * @param  {*}
 * @return {*}
 * @param {int} key_frame_index
 */
bool LoopClosing::CloudRegistration(int key_frame_index) {
	// 生成地图 
	CloudData::CLOUD_PTR map_cloud_ptr (new CloudData::CLOUD());
	Eigen::Matrix4f map_pose = Eigen::Matrix4f::Identity();
	JointMap(key_frame_index, map_cloud_ptr, map_pose);
	
	// 生成当前scan
	CloudData::CLOUD_PTR scan_cloud_ptr (new CloudData::CLOUD());
	Eigen::Matrix4f scan_pose = Eigen::Matrix4f::Identity();
	JointScan(scan_cloud_ptr, scan_pose);
	
	// 匹配
	Eigen::Matrix4f result_pose = Eigen::Matrix4f::Identity();
	Registration(map_cloud_ptr, scan_cloud_ptr, scan_pose, result_pose);
	
	// 计算相对姿态
	current_loop_pose_.pose = map_pose.inverse() * result_pose;

	// 判断是否有效
	if (registration_ptr_->GetFitnessScore() > fitness_score_limit_) {
		return false;
	}
	
	static int loop_close_cnt = 0;
	loop_close_cnt++;

	std::cout 	<< "检测到闭环" << loop_close_cnt << ": 帧" 
				<< current_loop_pose_.index0 << "<------>" << "帧" 
				<< current_loop_pose_.index1 << std::endl;


	return true;

}

/**
 * @description: 用于生成匹配时的历史帧附近局部地图点云
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::JointMap(int key_frame_index, CloudData::CLOUD_PTR& map_cloud_ptr, 
	Eigen::Matrix4f& map_pose) {
	map_pose = all_key_frames_.at(key_frame_index).pose;

	current_loop_pose_.index0 = all_key_frames_.at(key_frame_index).index;

	// 合成地图
	Eigen::Matrix4f pose_to_gnss = 
		map_pose * all_key_frames_.at(key_frame_index).pose.inverse();
	for (int i = key_frame_index - extend_frame_num_; 
		i < key_frame_index + extend_frame_num_; ++i) {
		std::string file_path = key_frames_path_ + 
			"/key_frame_" + std::to_string(all_key_frames_.at(i).index) + ".pcd";
		CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD());
		pcl::io::loadPCDFile(file_path, *cloud_ptr);
		Eigen::Matrix4f cloud_pose = pose_to_gnss * all_key_frames_.at(i).pose;
		pcl::transformPointCloud(*cloud_ptr, *cloud_ptr, cloud_pose);
		*map_cloud_ptr += *cloud_ptr;
	}
	map_filter_ptr_->Filter(map_cloud_ptr, map_cloud_ptr);
	return true;
}

/**
 * @description: 用于生成当前帧的点云
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::JointScan(CloudData::CLOUD_PTR& scan_cloud_ptr, Eigen::Matrix4f& scan_pose) {
	scan_pose = all_key_frames_.back().pose;
	current_loop_pose_.index1 = all_key_frames_.back().index;
	current_loop_pose_.time = all_key_frames_.back().time;

	std::string file_path = key_frames_path_ + 
		"/key_frame_" + std::to_string(all_key_frames_.back().index) + ".pcd";
	
	pcl::io::loadPCDFile(file_path, *scan_cloud_ptr);
	scan_filter_ptr_->Filter(scan_cloud_ptr, scan_cloud_ptr);
	return true;
}
/**
 * @description: 用于实现历史帧局部地图点云和当前扫描点云的匹配
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::Registration(	CloudData::CLOUD_PTR& map_cloud_ptr, 
					CloudData::CLOUD_PTR& scan_cloud_ptr,
					Eigen::Matrix4f& scan_pose,
					Eigen::Matrix4f& result_pose	) {
	// 点云匹配
	CloudData::CLOUD_PTR result_cloud_ptr (new CloudData::CLOUD());
	registration_ptr_->SetInputTarget(map_cloud_ptr);
	registration_ptr_->ScanMatch(scan_cloud_ptr, scan_pose, result_cloud_ptr, result_pose);
	
	return true;
}

/**
 * @description: public工具函数，对外接口
 * @param  {*}
 * @return {*}
 */
bool LoopClosing::HasNewLoopPose() {
	return has_new_loop_pose_;
}
/**
 * @description: public工具函数，对外接口
 * @param  {*}
 * @return {*}
 */
LoopPose& LoopClosing::GetCurrentLoopPose() {
	return current_loop_pose_;
}
}